#include "bmdcore.h"
typedef OpenMesh::Vec3f Vec3f;
typedef OpenMesh::PolyConnectivity::VertexVertexIter vvI;

BMDCore::BMDCore()
{
}

void BMDCore::Kernel(QOpenMesh *mesh, VH vHandle, float sigmac, float sigmas)
{
	OMesh* omesh = mesh->GetMesh();
    Vec3f vertex = omesh->point(vHandle);
    Vec3f normal = omesh->normal(vHandle);
    normal.normalize();
    vvI vvi = omesh->vv_iter(vHandle);
	float sum = 0;
	float normalizer = 0;
	QVector<Vec3f> neighbours;
	if (sigmac == 0)
		sigmac = 0.5;
	float offset = 0;
    for (;vvi;++vvi)
	{
		Vec3f q = omesh->point(vvi.handle());
		neighbours<<q;
		offset += (vertex-q).sqrnorm();
	}
	offset /= neighbours.size();
	if (sigmas == 0)
		sigmas = sqrtf(offset);
	for (int i=0;i<neighbours.size();i++)
	{
		Vec3f q = neighbours[i];
		float t = (vertex-q).norm();
        float h = dot(normal,q-vertex);
		float wc = expf(-t*t/(2*sigmac*sigmac));
		float ws = expf(-h*h/(2*sigmas*sigmas));
		sum += (wc*ws)*h;
		normalizer += (wc*ws);
    }
    omesh->set_point(vHandle, vertex + normal*(sum/normalizer));
}

void BMDCore::BMD( QOpenMesh* mesh, float sigmac, float sigmas )
{
	OMesh* omesh = mesh->GetMesh();
	VertexIter vi = omesh->vertices_begin();
    int count;
    for (;vi!=omesh->vertices_end();++vi,++count)
		Kernel(mesh, vi.handle(), sigmac, sigmas);
}
